{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "4f023b6c",
      "metadata": {},
      "source": [
        "# Pose2 SLAM Stress Test\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "colab_button",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/Pose2SLAMStressTest.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "colab_import",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n    import google.colab\n    %pip install --quiet gtsam-develop\nexcept ImportError:\n    pass"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "874ce32f",
      "metadata": {},
      "source": [
        "This stress test creates a large chain of poses to test GTSAM's performance on \n",
        "open-loop SLAM problems. The test:\n",
        " \n",
        "1. Creates N 3D poses with unit translation (1, 0, 0) between consecutive poses\n",
        "2. Adds random noise to the x value of each pose's translation\n",
        "3. Creates a prior factor for the first pose\n",
        "4. Adds N-1 between factors for consecutive poses\n",
        "5. Runs optimization using Levenberg-Marquardt\n",
        "\n",
        "This tests GTSAM's ability to handle large sparse factor graphs efficiently."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "id": "342b9c9f",
      "metadata": {},
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "import random\n",
        "import sys\n",
        "import time\n",
        "import gtsam"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "ceb4327b",
      "metadata": {},
      "source": [
        "## Stress Test Function"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "f0f4b5ed",
      "metadata": {},
      "outputs": [],
      "source": [
        "def test_gtsam(number_nodes):\n",
        "    \"\"\"\n",
        "    Test GTSAM on a large chain of poses.\n",
        "    \n",
        "    Args:\n",
        "        number_nodes: Number of poses to create in the chain\n",
        "    \"\"\"\n",
        "    print(f\"Creating stress test with {number_nodes} nodes...\")\n",
        "    \n",
        "    # Generate poses with random noise\n",
        "    poses = []\n",
        "    for i in range(number_nodes):\n",
        "        # Add random noise to x translation (between -0.05 and 0.05)\n",
        "        r = random.uniform(0, 1)\n",
        "        r = (r - 0.5) / 10 + i\n",
        "        \n",
        "        # Create 4x4 transformation matrix\n",
        "        # Translation: (r, 0, 0), no rotation\n",
        "        pose = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(r, 0, 0))\n",
        "        poses.append(pose)\n",
        "    \n",
        "    # Create factor graph\n",
        "    graph = gtsam.NonlinearFactorGraph()\n",
        "    \n",
        "    # Prior factor for the first pose\n",
        "    prior_model = gtsam.noiseModel.Isotropic.Variance(6, 1e-4)\n",
        "    first_pose = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0, 0, 0))\n",
        "    graph.add(gtsam.PriorFactorPose3(0, first_pose, prior_model))\n",
        "    \n",
        "    # VO (Visual Odometry) noise model\n",
        "    vo_covariance_model = gtsam.noiseModel.Isotropic.Variance(6, 1e-3)\n",
        "    relative_motion = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))\n",
        "    \n",
        "    # Add between factors for consecutive poses\n",
        "    for i in range(number_nodes - 1):\n",
        "        graph.add(gtsam.BetweenFactorPose3(i, i + 1, relative_motion, vo_covariance_model))\n",
        "    \n",
        "    print(f\"Created factor graph with {graph.size()} factors\")\n",
        "    \n",
        "    # Initial values\n",
        "    initial = gtsam.Values()\n",
        "    for i in range(number_nodes):\n",
        "        initial.insert(i, poses[i])\n",
        "    \n",
        "    # Optimization parameters\n",
        "    params = gtsam.LevenbergMarquardtParams()\n",
        "    params.setVerbosity(\"ERROR\")\n",
        "    params.setOrderingType(\"METIS\")\n",
        "    params.setLinearSolverType(\"MULTIFRONTAL_CHOLESKY\")\n",
        "    \n",
        "    # Run optimization\n",
        "    print(\"Starting optimization...\")\n",
        "    start_time = time.time()\n",
        "    \n",
        "    optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)\n",
        "    result = optimizer.optimize()\n",
        "    \n",
        "    end_time = time.time()\n",
        "    optimization_time = end_time - start_time\n",
        "    \n",
        "    print(f\"Optimization completed in {optimization_time:.3f} seconds\")\n",
        "    print(f\"Initial error: {graph.error(initial):.6f}\")\n",
        "    print(f\"Final error: {graph.error(result):.6f}\")\n",
        "    \n",
        "    return result, optimization_time"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "b0063563",
      "metadata": {},
      "source": [
        "## Main Execution"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "id": "ed1b11a3",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Invalid number of nodes. Using default value of 1000.\n",
            "Running Pose2 SLAM Stress Test with 1000 nodes\n",
            "Creating stress test with 1000 nodes...\n",
            "Created factor graph with 1000 factors\n",
            "Starting optimization...\n",
            "Initial error: 810.02902132Optimization completed in 0.020 seconds\n",
            "Initial error: 810.029021\n",
            "Final error: 0.000000\n",
            "\n",
            "Stress test completed successfully!\n",
            "Performance: 49290.2 nodes per second\n",
            "\n",
            "newError: 7.18522916506e-11\n",
            "errorThreshold: 7.18522916506e-11 > 0\n",
            "absoluteDecrease: 810.02902132 >= 1e-05\n",
            "relativeDecrease: 1 >= 1e-05\n",
            "newError: 1.1004643035e-17\n",
            "errorThreshold: 1.1004643035e-17 > 0\n",
            "absoluteDecrease: 7.1852280646e-11 < 1e-05\n",
            "relativeDecrease: 0.999999846844 >= 1e-05\n",
            "converged\n",
            "errorThreshold: 1.1004643035e-17 <? 0\n",
            "absoluteDecrease: 7.1852280646e-11 <? 1e-05\n",
            "relativeDecrease: 0.999999846844 <? 1e-05\n",
            "iterations: 2 >? 100\n"
          ]
        }
      ],
      "source": [
        "def main():\n",
        "    \"\"\"Main function to run the stress test.\"\"\"\n",
        "    # Default number of nodes if not specified\n",
        "    number_nodes = 1000\n",
        "    \n",
        "    # Check if number of nodes is provided as command line argument\n",
        "    if len(sys.argv) > 1:\n",
        "        try:\n",
        "            number_nodes = int(sys.argv[1])\n",
        "        except ValueError:\n",
        "            print(\"Invalid number of nodes. Using default value of 1000.\")\n",
        "    \n",
        "    print(f\"Running Pose2 SLAM Stress Test with {number_nodes} nodes\")\n",
        "    \n",
        "    # Set random seed for reproducibility\n",
        "    random.seed(42)\n",
        "    np.random.seed(42)\n",
        "    \n",
        "    # Run the stress test\n",
        "    result, opt_time = test_gtsam(number_nodes)\n",
        "    \n",
        "    print(\"\\nStress test completed successfully!\")\n",
        "    print(f\"Performance: {number_nodes / opt_time:.1f} nodes per second\")\n",
        "\n",
        "main()"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "9c5e3ae7",
      "metadata": {},
      "source": [
        "## Interactive Example\n",
        "\n",
        "You can run the stress test with different numbers of nodes to see how \n",
        "GTSAM performs with increasing problem size."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "id": "1fec1857",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Running stress tests with different problem sizes:\n",
            "==================================================\n",
            "\n",
            "Testing with 100 nodes:\n",
            "Creating stress test with 100 nodes...\n",
            "Created factor graph with 100 factors\n",
            "Starting optimization...\n",
            "Optimization completed in 0.007 seconds\n",
            "Initial error: 73.785267\n",
            "Final error: 0.000000\n",
            "Rate: 14021.2 nodes/second\n",
            "\n",
            "Testing with 500 nodes:\n",
            "Creating stress test with 500 nodes...\n",
            "Initial error: 73.7852667959\n",
            "Created factor graph with 500 factors\n",
            "Starting optimization...\n",
            "newError: 3.19648178005e-14\n",
            "errorThreshold: 3.19648178005e-14 > 0\n",
            "absoluteDecrease: 73.7852667959 >= 1e-05\n",
            "relativeDecrease: 1 >= 1e-05\n",
            "newError: 3.26892942612e-25\n",
            "errorThreshold: 3.26892942612e-25 > 0\n",
            "absoluteDecrease: 3.19648178002e-14 < 1e-05\n",
            "relativeDecrease: 0.99999999999 >= 1e-05\n",
            "converged\n",
            "errorThreshold: 3.26892942612e-25 <? 0\n",
            "absoluteDecrease: 3.19648178002e-14 <? 1e-05\n",
            "relativeDecrease: 0.99999999999 <? 1e-05\n",
            "iterations: 2 >? 100\n",
            "Initial error: 403.93736772\n",
            "newError: 1.44414083181e-12\n",
            "errorThreshold: 1.44414083181e-12 > 0\n",
            "absoluteDecrease: 403.93736772 >= 1e-05\n",
            "relativeDecrease: 1 >= 1e-05\n",
            "newError: 1.00814883652e-20\n",
            "errorThreshold: 1.00814883652e-20 > 0\n",
            "absoluteDecrease: 1.44414082173e-12 < 1e-05\n",
            "relativeDecrease: 0.999999993019 >= 1e-05\n",
            "converged\n",
            "errorThreshold: 1.00814883652e-20 <? 0\n",
            "absoluteDecrease: 1.44414082173e-12 <? 1e-05\n",
            "relativeDecrease: 0.999999993019 <? 1e-05\n",
            "iterations: 2 >? 100\n",
            "Optimization completed in 0.023 seconds\n",
            "Initial error: 403.937368\n",
            "Final error: 0.000000\n",
            "Rate: 21542.6 nodes/second\n",
            "\n",
            "Testing with 1000 nodes:\n",
            "Creating stress test with 1000 nodes...\n",
            "Created factor graph with 1000 factors\n",
            "Starting optimization...\n",
            "Initial error: 810.02902132\n",
            "newError: 7.18522916506e-11\n",
            "errorThreshold: 7.18522916506e-11 > 0\n",
            "absoluteDecrease: 810.02902132 >= 1e-05\n",
            "relativeDecrease: 1 >= 1e-05\n",
            "newError: 1.1004643035e-17\n",
            "errorThreshold: 1.1004643035e-17 > 0\n",
            "absoluteDecrease: 7.1852280646e-11 < 1e-05\n",
            "relativeDecrease: 0.999999846844 >= 1e-05\n",
            "converged\n",
            "errorThreshold: 1.1004643035e-17 <? 0\n",
            "absoluteDecrease: 7.1852280646e-11 <? 1e-05\n",
            "relativeDecrease: 0.999999846844 <? 1e-05\n",
            "iterations: 2 >? 100\n",
            "Optimization completed in 0.041 seconds\n",
            "Initial error: 810.029021\n",
            "Final error: 0.000000\n",
            "Rate: 24680.9 nodes/second\n",
            "\n",
            "Testing with 2000 nodes:\n",
            "Creating stress test with 2000 nodes...\n",
            "Created factor graph with 2000 factors\n",
            "Starting optimization...\n",
            "Initial error: 1629.41004549\n",
            "newError: 6.70560309189e-11\n",
            "errorThreshold: 6.70560309189e-11 > 0\n",
            "absoluteDecrease: 1629.41004549 >= 1e-05\n",
            "relativeDecrease: 1 >= 1e-05\n",
            "newError: 1.51418327449e-16\n",
            "errorThreshold: 1.51418327449e-16 > 0\n",
            "absoluteDecrease: 6.70558795006e-11 < 1e-05\n",
            "relativeDecrease: 0.999997741913 >= 1e-05\n",
            "converged\n",
            "errorThreshold: 1.51418327449e-16 <? 0\n",
            "absoluteDecrease: 6.70558795006e-11 <? 1e-05\n",
            "relativeDecrease: 0.999997741913 <? 1e-05\n",
            "iterations: 2 >? 100\n",
            "Optimization completed in 0.040 seconds\n",
            "Initial error: 1629.410045\n",
            "Final error: 0.000000\n",
            "Rate: 50276.6 nodes/second\n"
          ]
        }
      ],
      "source": [
        "test_sizes = [100, 500, 1000, 2000]\n",
        "\n",
        "print(\"Running stress tests with different problem sizes:\")\n",
        "print(\"=\" * 50)\n",
        "\n",
        "for size in test_sizes:\n",
        "    print(f\"\\nTesting with {size} nodes:\")\n",
        "    random.seed(42)  # Ensure reproducible results\n",
        "    np.random.seed(42)\n",
        "    \n",
        "    result, opt_time = test_gtsam(size)\n",
        "    print(f\"Rate: {size / opt_time:.1f} nodes/second\")\n"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": ".venv",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.9"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}